// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CHOMPTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Geometry.SDF;
    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Hardware.Mico;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.MotionPlanning;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using ShoNS.Array;

    /// <summary>
    /// A test class for CHOMP motion planning
    /// </summary>
    [TestClass]
    public class CHOMPTest
    {
        /// <summary>
        /// Test CHOMP on a table scene with the KUKA arm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CHOMPMotionPlanningTest_KUKA_TableScene()
        {
            // Get the KUKA kinematic chain
            KinematicChain kc = KukaLBR4KinematicConfigurations.CreateKuka7DOFKinematicChain();

            // CREATE AN ENVIRONMENT WITH A TABLE AT Z = 0 
            TableEnvironment tableEnv = new TableEnvironment();
            tableEnv.TableHeight = 0.0; // table is at z = 0

            // Create an initial trajectory that goes through the table
            DoubleArray start = DoubleArray.From(new double[] { 0, MathConstants.PIOverTwo, 0, 0, 0, 0, 0 }).Transpose();
            DoubleArray midPoint = DoubleArray.From(new double[] { MathConstants.PIOverFour, MathConstants.PIOverTwo + MathConstants.PIOverFour, 0, -MathConstants.TwoPI / 12, 0, 0, 0 }).Transpose();
            DoubleArray goal = DoubleArray.From(new double[] { MathConstants.PIOverTwo, MathConstants.PIOverTwo, 0, 0, 0, 0, 0 }).Transpose();

            // Linearly interpolate between the three points
            DoubleArray pt1 = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(start, midPoint, 51);
            DoubleArray pt2 = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(midPoint, goal, 51);
            DoubleArray initTrajectory = new DoubleArray(pt1.size0, pt1.size1 + pt2.size1 - 1);
            for (int j = 0; j < pt1.size0; j++)
            {
                for (int i = 0; i < pt1.size1; i++)
                {
                    initTrajectory[j, i] = pt1[j, i];
                }

                for (int i = 1; i < pt2.size1; i++)
                {
                    initTrajectory[j, (i - 1) + pt1.size1] = pt2[j, i];
                }
            }

            // Create an instance of the CHOMP parameters class with test specific values
            CHOMPparameters initParams = new CHOMPparameters(
                numPoints: 101,
                stepSize: 1.0 / 5000,
                maxTime: 5.0,
                useSelfCollisionInConvergenceTesting: true,
                exitOnNoCollision: true);

            // Create a planner instance
            MotionPlanCHOMP planner = new MotionPlanCHOMP(
                kc,
                initParams,
                tableEnv.ComputeSphereCostandGradient);

            // Run CHOMP
            DoubleArray final_trajectory = planner.RunCHOMPOptimization(initTrajectory);

            // Make sure that CHOMP can find a collision free path
            Assert.AreEqual(4, planner.ConvergenceType, "CHOMP fails to find collision free path on test scene with a table at Z=0 with KUKA arm!"); // convergence type = 4 means that we are out of collision
        }

        /// <summary>
        /// Test CHOMP on an SDF scene where it does not find a collision free path
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [DeploymentItem(@"Data\TableTop3", @"Data")]
        public void CHOMPMotionPlanningTest_KUKA_SDFScene_DoesNotFindCollisionFreePath()
        {
            // Get the KUKA kinematic chain
            KinematicChain kc = KukaLBR4KinematicConfigurations.CreateKuka7DOFKinematicChain();

            // Load a test SDF from file (has a table top scene with a monkey and a buddha)
            SDFEnvironment sdfEnv =  new SDFEnvironment();
            sdfEnv.Sdf = SDFIO.LoadSDF(@"Data\", "TableTop3");

            // Create an instance of the CHOMP parameters class with test specific values
            CHOMPparameters initParams = new CHOMPparameters(
                numPoints: 101,
                stepSize: 1.0 / 250,
                maxTime: 5.0,
                useSelfCollisionInConvergenceTesting: true,
                exitOnNoCollision: true);

            // Create an initial trajectory that passes through both the obstacles in the SDF
            DoubleArray currentJoint = DoubleArray.From(new double[] { -Math.PI * 0.75, -Math.PI / 6, -Math.PI / 4, 0, 0, 0, 0 }).Transpose();
            DoubleArray targetJoint = DoubleArray.From(new double[] { Math.PI * 2 / 3, -Math.PI / 3, Math.PI / 2, 0, 0, 0, 0 }).Transpose();
            DoubleArray initTrajectory = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(currentJoint, targetJoint, initParams.NumPoints);

            // Create a planner instance
            MotionPlanCHOMP planner = new MotionPlanCHOMP(
                kc,
                initParams,
                sdfEnv.ComputeSphereCostandGradient);

            // Run CHOMP
            DoubleArray final_trajectory = planner.RunCHOMPOptimization(initTrajectory);

            // CHOMP cannot find a collision free path in this case. 
            // The robot ends up passing through the ear of the monkey even after many optimization iterations
            Assert.AreNotEqual(4, planner.ConvergenceType, "CHOMP succeeds on SDF test scene while it is supposed to fail with KUKA arm!"); // convergence type = 4 means that we are out of collision
        }

        /// <summary>
        /// Test CHOMP on a scene with spherical obstacles
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CHOMPMotionPlanningTest_KUKA_SphereScene()
        {
            // Get the KUKA kinematic chain
            KinematicChain kc = KukaLBR4KinematicConfigurations.CreateKuka7DOFKinematicChain();

            // Generate spherical obstacles - TWO SPHERES COLLIDING WITH THE INITIAL TRAJECTORY
            SphereEnvironment sphereEnv = new SphereEnvironment();
            List<GeometryElement3D> objects = new List<GeometryElement3D>();
            objects.Add(new SphereElement(new Vector3(-0.05, 0, 1.1), 0.2));
            objects.Add(new SphereElement(new Vector3(-0.35, 0.1, 0.8), 0.2));
            sphereEnv.Obstacles = new GeometryElementGroup3D(objects); // add obstacles to the environment

            // Create an instance of the CHOMP parameters class with test specific values
            CHOMPparameters initParams = new CHOMPparameters(
                numPoints: 101,
                stepSize: 1.0 / 250,
                maxTime: 5.0,
                useSelfCollisionInConvergenceTesting: true,
                exitOnNoCollision: true);

            // GENERATE A TRAJECTORY THAT COLLIDES WITH THE OBSTACLES
            DoubleArray targetJoint = DoubleArray.From(new double[] { 0, MathConstants.PIOverTwo, 0, 0, 0, 0, 0 }).Transpose();
            DoubleArray currentJoint = DoubleArray.From(new double[] { 0, -MathConstants.PIOverTwo, 0, 0, 0, 0, 0 }).Transpose();
            DoubleArray initTrajectory = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(currentJoint, targetJoint, initParams.NumPoints);

            // Create a planner instance
            MotionPlanCHOMP planner = new MotionPlanCHOMP(
                kc,
                initParams,
                sphereEnv.ComputeSphereCostandGradient);

            // Run CHOMP
            DoubleArray final_trajectory = planner.RunCHOMPOptimization(initTrajectory);

            // CHOMP finds a collision free path in this case
            Assert.AreEqual(4, planner.ConvergenceType, "CHOMP fails to find collision free path on spherical obstacle test scene with KUKA arm!"); // convergence type = 4 means that we are out of collision
        }

        /// <summary>
        /// Test CHOMP on a scene with spherical obstacles with the MICO. 
        /// For MICO, do not use self collision check for convergence testing as it always seems to be in self-collision from our sphere model based testing.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CHOMPMotionPlanningTest_MICO_SphereScene()
        {
            // Get the mico kinematic chain
            KinematicChain kc = MicoKinematicConfigurations.CreateMicoKinematicChain();

            // Generate spherical obstacles - TWO SPHERES COLLIDING WITH THE INITIAL TRAJECTORY
            SphereEnvironment sphereEnv = new SphereEnvironment();
            List<GeometryElement3D> objects = new List<GeometryElement3D>();
            objects.Add(new SphereElement(new Vector3(-0.05, 0, 1.1), 0.2));
            objects.Add(new SphereElement(new Vector3(-0.35, 0.1, 0.8), 0.2));
            sphereEnv.Obstacles = new GeometryElementGroup3D(objects); // add obstacles to the environment

            // Create an instance of the CHOMP parameters class with test specific values
            // Key params: large step size (1/50) and do not use self collision for convergence testing for MICO
            // Empirically found out that step size needs to be large for this case.
            CHOMPparameters initParams = new CHOMPparameters(
                numPoints: 101,
                stepSize: 1.0 / 50,
                maxTime: 5.0,
                useSelfCollisionInConvergenceTesting: false,
                exitOnNoCollision: true);

            // GENERATE A TRAJECTORY THAT COLLIDES WITH THE OBSTACLES
            DoubleArray targetJoint = DoubleArray.From(new double[] { 0, MathConstants.PIOverTwo, Math.PI, 0, 0, 0 }).Transpose();
            DoubleArray currentJoint = DoubleArray.From(new double[] { 0, Math.PI + MathConstants.PIOverTwo, Math.PI, 0, 0, 0 }).Transpose();
            DoubleArray initTrajectory = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(currentJoint, targetJoint, initParams.NumPoints);

            // Create a planner instance
            MotionPlanCHOMP planner = new MotionPlanCHOMP(
                kc,
                initParams,
                sphereEnv.ComputeSphereCostandGradient);

            // Run CHOMP
            DoubleArray final_trajectory = planner.RunCHOMPOptimization(initTrajectory);

            // CHOMP finds a collision free path in this case
            Assert.AreEqual(4, planner.ConvergenceType, "CHOMP fails to find collision free path on spherical obstacle test scene with MICO arm!"); // convergence type = 4 means that we are out of collision
        }

        /// <summary>
        /// Test CHOMP on a table scene with the MICO arm. CHOMP finds a collision free path here.
        /// For MICO, do not use self collision check for convergence testing as it always seems to be in self-collision from our sphere model based testing.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CHOMPMotionPlanningTest_MICO_TableScene()
        {
            // Get the mico kinematic chain
            KinematicChain kc = MicoKinematicConfigurations.CreateMicoKinematicChain();

            // CREATE AN ENVIRONMENT WITH A TABLE AT Z = 0 
            TableEnvironment tableEnv = new TableEnvironment();
            tableEnv.TableHeight = 0.0; // table is at z = 0

            // Create an initial trajectory that goes through the table
            DoubleArray start = DoubleArray.From(new double[] { 0, MathConstants.PIOverTwo, Math.PI, 0, 0, 0 }).Transpose();
            DoubleArray midPoint = DoubleArray.From(new double[] { MathConstants.PIOverFour, MathConstants.PIOverTwo - MathConstants.PIOverFour, Math.PI, -MathConstants.TwoPI / 12, 0, 0 }).Transpose();
            DoubleArray goal = DoubleArray.From(new double[] { MathConstants.PIOverTwo, MathConstants.PIOverTwo, Math.PI, 0, 0, 0 }).Transpose();

            // Linearly interpolate between the three points
            DoubleArray pt1 = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(start, midPoint, 51);
            DoubleArray pt2 = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(midPoint, goal, 51);
            DoubleArray initTrajectory = new DoubleArray(pt1.size0, pt1.size1 + pt2.size1 - 1);
            for (int j = 0; j < pt1.size0; j++)
            {
                for (int i = 0; i < pt1.size1; i++)
                {
                    initTrajectory[j, i] = pt1[j, i];
                }

                for (int i = 1; i < pt2.size1; i++)
                {
                    initTrajectory[j, (i - 1) + pt1.size1] = pt2[j, i];
                }
            }

            // Create an instance of the CHOMP parameters class with test specific values
            // Key params: Do not use self collision for convergence testing for MICO
            CHOMPparameters initParams = new CHOMPparameters(
                numPoints: 101,
                stepSize: 1.0 / 5000,
                maxTime: 5.0,
                useSelfCollisionInConvergenceTesting: false,
                exitOnNoCollision: true);

            // Create a planner instance
            MotionPlanCHOMP planner = new MotionPlanCHOMP(
                kc,
                initParams,
                tableEnv.ComputeSphereCostandGradient);

            // Run CHOMP
            DoubleArray final_trajectory = planner.RunCHOMPOptimization(initTrajectory);

            // Make sure that CHOMP can find a collision free path
            Assert.AreEqual(4, planner.ConvergenceType, "CHOMP fails to find collision free path on test scene with a table at Z=0 with MICO arm!"); // convergence type = 4 means that we are out of collision
        }

        /// <summary>
        /// Test CHOMP on an SDF scene with the MICO arm where it does not find a collision free path
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [DeploymentItem(@"Data\TableTop3", @"Data")]
        public void CHOMPMotionPlanningTest_MICO_SDFScene_DoesNotFindCollisionFreePath()
        {
            // Get the mico kinematic chain
            KinematicChain kc = MicoKinematicConfigurations.CreateMicoKinematicChain();

            // Load a test SDF from file (has a table top scene with a monkey and a buddha)
            SDFEnvironment sdfEnv = new SDFEnvironment();
            sdfEnv.Sdf = SDFIO.LoadSDF(@"Data\", "TableTop3");

            // Create an instance of the CHOMP parameters class with test specific values
            // Key params: Do not use self collision for convergence testing for MICO
            CHOMPparameters initParams = new CHOMPparameters(
                numPoints: 101,
                stepSize: 1.0 / 50,
                maxTime: 5.0,
                envCollisionTolerance: 0.05,
                useSelfCollisionInConvergenceTesting: false,
                exitOnNoCollision: true);

            // Create an initial trajectory that passes through both the obstacles in the SDF
            DoubleArray currentJoint = DoubleArray.From(new double[] { Math.PI - Math.PI / 4, Math.PI / 2, Math.PI, 0, 0, 0 }).Transpose();
            DoubleArray targetJoint = DoubleArray.From(new double[] { Math.PI / 4, Math.PI, Math.PI / 2, 0, 0, 0 }).Transpose();
            DoubleArray initTrajectory = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(currentJoint, targetJoint, initParams.NumPoints);

            // Create a planner instance
            MotionPlanCHOMP planner = new MotionPlanCHOMP(
                kc,
                initParams,
                sdfEnv.ComputeSphereCostandGradient);

            // Run CHOMP
            DoubleArray final_trajectory = planner.RunCHOMPOptimization(initTrajectory);

            // CHOMP cannot find a collision free path in this case (een though the final trajectory looks out of collision. NEED to check
            // The robot ends up passing through the ear of the monkey even after many optimization iterations
            Assert.AreNotEqual(4, planner.ConvergenceType, "CHOMP succeeds on SDF test scene while it is supposed to fail with MICO arm!"); // convergence type = 4 means that we are out of collision
        }

        /// <summary>
        /// Test CHOMP on a scene with spherical obstacles where the optimization has to handle joint limit violations.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CHOMPMotionPlanningTest_KUKA_SphereScene_WithJointLimitViolations()
        {
            // Get the KUKA kinematic chain
            KinematicChain kc = KukaLBR4KinematicConfigurations.CreateKuka7DOFKinematicChain();

            // Generate spherical obstacles - TWO SPHERES COLLIDING WITH THE INITIAL TRAJECTORY
            SphereEnvironment sphereEnv = new SphereEnvironment();
            List<GeometryElement3D> objects = new List<GeometryElement3D>();
            objects.Add(new SphereElement(new Vector3(-0.05, 0, 1.1), 0.25));
            objects.Add(new SphereElement(new Vector3(-0.35, 0.1, 0.8), 0.2));
            sphereEnv.Obstacles = new GeometryElementGroup3D(objects); // add obstacles to the environment

            // Create an instance of the CHOMP parameters class with test specific values
            // The large step size makes the algorithm update the trajectory by a lot, causing it to violate joint limits
            // In our test configurations, the robot joints end ub being very close to each other, so we reduce the self collision tolerance to 1 cm
            CHOMPparameters initParams = new CHOMPparameters(
                numPoints: 101,
                stepSize: 1.0 / 20,
                maxTime: 5.0,
                selfCollisionTolerance: 0.01,
                useSelfCollisionInConvergenceTesting: true,
                exitOnNoCollision: true);

            // GENERATE A TRAJECTORY THAT COLLIDES WITH THE OBSTACLES
            DoubleArray targetJoint = DoubleArray.From(new double[] { 0, MathConstants.PIOverTwo, -1.4, 0, 0, 0, 0 }).Transpose();
            DoubleArray currentJoint = DoubleArray.From(new double[] { 0, -2.07, -2.96, 2.07, 0, 0, 0 }).Transpose();
            DoubleArray initTrajectory = MotionPlanCHOMP.GenerateTrajectoryUsingLinearInterpolation(currentJoint, targetJoint, initParams.NumPoints);

            // Create a planner instance
            MotionPlanCHOMP planner = new MotionPlanCHOMP(
                kc,
                initParams,
                sphereEnv.ComputeSphereCostandGradient);

            // Run CHOMP
            DoubleArray final_trajectory = planner.RunCHOMPOptimization(initTrajectory);

            // CHOMP finds a collision free path in this case
            Assert.AreEqual(4, planner.ConvergenceType, "CHOMP fails to find collision free path on spherical obstacle test scene with KUKA arm!"); // convergence type = 4 means that we are out of collision
        }
    }
}
